#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTSMUX,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
void initializeRobot()
{
  wait1Msec(1000);
  ClearTimer(T1);
  ClearTimer(T2);
  servoChangeRate[servo1] = 1;
  servoChangeRate[servo2] = 1;
  servoChangeRate[servo3] = 1;
  servo[servo1] = 250;
  servo[servo6] = 160;
  servo[servo3] = 95;
  servo[servo5] = 10;
  servo[servo4] = 245;
  wait1Msec(1000);
  return;
}

task main()
{
  initializeRobot();
  waitForStart();

  servo[servo6] = 10;
  servo[servo3] = 245;
  motor[motorE] = 0;
  motor[motorD] = 0;
  nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;
  nMotorEncoderTarget[motorE] = 10000;
  nMotorEncoderTarget[motorD] = 10000;
  motor[motorD] = 80;
  motor[motorE] = 80;
  ClearTimer(T2);
  while (nMotorRunState[motorE] != runStateIdle && nMotorRunState[motorD] != runStateIdle && time1[T2] < 5000);
}
